#!/usr/bin/env python
# -*- coding:utf-8 -*-

# Autor:	Roger
# Date:		20210602
# Version:	0.1
#Thanks for origin Autor's Ingmar Stape 
# 这个模块通过PWM控制L298N电机驱动两个马达的情况。范例：
# import l298n
# HBridge = l298n.HBridge(19, 16, 26, 21, 13, 20)
# HBridge.Init()

import RPi.GPIO as io
from enum import Enum

#定义一些常量
PWM_MAX = 100  # PWM最大占空比，100表示100%
PWM_HZ  = 100  # PWM频率，单位赫兹 Hz，表示1秒变化100次，即每个波长是10毫秒

# 方向
class Direction(Enum):
    STOP = 0
    FORWARD = 1
    BACKWARD = 2

# 左右马达
class Steer(Enum):
    LEFT = 1
    RIGHT = 2

class HBridge(object):
    def __init__(self, left_pin1, left_pin2, right_pin1, right_pin2, leftpwm_pin, rightpwm_pin):
        io.setmode(io.BCM)
        io.setwarnings(False)
        self.left_pin1 = left_pin1
        self.left_pin2 = left_pin2
        self.right_pin1 = right_pin1
        self.right_pin2 = right_pin2
        self.leftpwm_pin = leftpwm_pin
        self.rightpwm_pin = rightpwm_pin        
        self._initGPIO()        
        self._initPWM()        

    def _initGPIO(self):
        io.setup(self.right_pin1, io.OUT, initial=io.LOW)
        io.setup(self.right_pin2, io.OUT, initial=io.LOW)
        io.setup(self.left_pin1, io.OUT, initial=io.LOW)
        io.setup(self.left_pin2, io.OUT, initial=io.LOW)
        io.setup(self.leftpwm_pin, io.OUT, initial=io.LOW)
        io.setup(self.rightpwm_pin, io.OUT, initial=io.LOW)

    def _initPWM(self): 
        self.leftpwm = io.PWM(self.leftpwm_pin, PWM_HZ)        
        self.leftpwm.start(0)
        self.leftpwm.ChangeDutyCycle(0)
        self.rightpwm = io.PWM(self.rightpwm_pin, PWM_HZ)
        self.rightpwm.start(0)
        self.rightpwm.ChangeDutyCycle(0)
    
    def resetMotorGPIO(self):
        io.output(self.left_pin1, False)
        io.output(self.left_pin2, False)
        io.output(self.right_pin1, False)
        io.output(self.right_pin2, False)

# 设置马达前进后退模式
# steer，要修改模式的马达
# direction，前进或后退模式
    def setMotorMode(self, steer, direction):
        if steer == Steer.LEFT:
            if direction == Direction.BACKWARD:
                io.output(self.left_pin1, True)
                io.output(self.left_pin2, False)
            elif  direction == Direction.FORWARD:
                io.output(self.left_pin1, False)
                io.output(self.left_pin2, True)
            else:
                io.output(self.left_pin1, False)
                io.output(self.left_pin2, False)
        elif steer == Steer.RIGHT:
            if direction == Direction.BACKWARD:
                io.output(self.right_pin1, False)
                io.output(self.right_pin2, True)		
            elif direction == Direction.FORWARD:
                io.output(self.right_pin1, True)
                io.output(self.right_pin2, False)		
            else:
                io.output(self.right_pin1, False)
                io.output(self.right_pin2, False)
        else:
            self.resetMotorGPIO()

# 设置马达功率，从-1到1的小数
# This is a short explanation for a better understanding:
# SetMotor(0)     -> left motor is stopped
# SetMotorLeft(0.75)  -> left motor moving forward at 75% power
# SetMotorLeft(-0.5)  -> left motor moving reverse at 50% power
# SetMotorLeft(1)     -> left motor moving forward at 100% power
    def setMotor(self, motor, power):
        pwm = round(abs(PWM_MAX * power))
        pwm = 100 if pwm > 100 else pwm
        if power < 0:
            self.setMotorMode(motor, Direction.BACKWARD)
        elif power > 0:
            self.setMotorMode(motor, Direction.FORWARD)
        else:
            self.setMotorMode(motor, Direction.STOP)
            pwm = 0
        
        if motor == Steer.LEFT:
            self.leftpwm.ChangeDutyCycle(pwm)
        elif motor == Steer.RIGHT:
            self.rightpwm.ChangeDutyCycle(pwm)

# 清理所有GPIO设置并退出
    def exit(self):
        self.resetMotorGPIO()
        io.cleanup()
